#include "./SYSTEM/sys/sys.h"
#include "./SYSTEM/usart/usart.h"
#include "./SYSTEM/delay/delay.h"
#include "./BSP/OLED/OLED.h"
#include "./BSP/MPU6050/mpu6050.h"
uint8_t ret;
uint8_t ID;
MPU6050_Data mpu6050_data;
EulerAngle angle;
int main()
{
    HAL_Init();
    delay_init(168);
    sys_stm32_clock_init(336,8,2,7);
    usart_init(115200);
    OLED_Init();
    MPU6050_Init();
    OLED_Printf(0,32,OLED_8X16,"Hello if");
    ID = MPU6050_GetID();
    //OLED_Printf(0, 1, OLED_8X16, "OLED");
    OLED_Printf(0, 0,OLED_8X16, "ID: %x", ID);
    OLED_Update();

    while(1)
    {
        OLED_Printf(0, 0,OLED_8X16, "ID: %x", ID);
        MPU6050_getraw(&mpu6050_data);
        MPU6050_Get_EulerAngle(&angle);
        OLED_Printf(0,16,OLED_8X16,"%.2f",angle.Roll);
        OLED_Printf(0,32,OLED_8X16,"%.2f",angle.Pitch);
        OLED_Printf(0,48,OLED_8X16,"%.2f",angle.Yaw);
        OLED_Update();
        delay_ms(100);
    }
}

